using System;
using FPLibrary;
using UnityEngine;

namespace PathX
{
    public class ManhattanDistance : IMoveCost
    {
        public int baseMoveCost { get; private set; }
        public ManhattanDistance(int cellMoveCost)
        {
            this.baseMoveCost = cellMoveCost;
        }
        
        public int GetMoveCost(IPositioned current, IPositioned other)
        {
            var dx = Fix64.FastAbs(current.position.x - other.position.x);
            var dz = Fix64.FastAbs(current.position.z - other.position.z);
            var dy = Fix64.FastAbs(current.position.y - other.position.y);
            
            return Mathf.RoundToInt((float)(baseMoveCost * (dx + dz + dy)));
        }
        
        public int GetHeuristic(IPositioned current, IPositioned goal)
        {
            var dx = Fix64.FastAbs(current.position.x - goal.position.x);
            var dz = Fix64.FastAbs(current.position.z - goal.position.z);
            var dy = Fix64.FastAbs(current.position.y - goal.position.y);

            return Mathf.RoundToInt((float)(baseMoveCost * (dx + dz + dy)));
        }
    }
}